#include "WProgram.h"
#include "Robot_CLI.h"

//Public methods

//Constructors
Robot_CLI::Robot_CLI( ){}//Constructor doesn't need to do anything

//Initializers
void Robot_CLI::startComm( int speed )
{

	//Create head node and share address with _current and _tail
	_head = (node) malloc( sizeof( struct cmd_lst ) );
	_current = _head;
	_tail = _head;

	//Build the head node.
	for( int i = 0; _packet_size >= i; i++ )
	{
		_head->packet[ i ] = -1;//Set all values in packet to -1.
	}
	_head->next = NULL;
	_head->nodeNum = 0;

	//Start the Serial port.
	Serial.begin( speed );

	//Wait for a character to arrive before beginning.
	//Might want to make this be a specific character.
	if ( _establishContact() )
	{
		Serial.println( "Ready" );
		Serial.flush();//flush out the dummy characters.
	}
}

int Robot_CLI::getCommand( int commandRef[], const unsigned int size,
					const unsigned char token )
{
	if( _packet_size >= size && Serial.available() > 0 )
	{
		int i = 0;

		do{
			if( _packet_size <= i ){ break; }

			_tail->packet[ i ] = Serial.read();
			commandRef[ i ] = _tail->packet[ i ];
			i++;

		}while( Serial.available() > 0 );
		
		//Always have a next node ready
		//Never exceed _cmd_lst_size
		if( _tail->nodeNum == _cmd_lst_size )
		{
			//Add max list size code later.
			//For now just delete the list and start over.
			_delCmdLst();
		}
		else
		{
			_current = _tail; //Increment current to tail

			//create new tail
			_tail = (node) malloc( sizeof( struct cmd_lst ) );
			
 			//Set current next pointer to new tail
			_current->next = _tail;

			//Build new tail
			for( int i = 0; _packet_size >= i; i++)
			{
				_tail->packet[ i ] = -1;//Set packet to all -1.
			}
			_tail->next = NULL;
			_tail->nodeNum = ( _current->nodeNum + 1 );
		}
	}
	else
	{
		return( -1 ); //Will deal with bigger packet case later
	}

	return( 0 );
}

int Robot_CLI::sendCommand( int commandRef[], 
				const unsigned int size,
				const unsigned char token )
{

	//Not going to keep track of sent commands at this point.
	for( int i = 0; i < size; i++)
	{
		Serial.print( commandRef[ i ] );
		//Token character
		if( size - 1 > i )
		{
			Serial.print( token );
		}
	}
	Serial.println();

	return( 0 );//Will set up for receiving a hash confirmation later
}

void Robot_CLI::sendCmdLog( const unsigned char token )
{
	node seeker;
	for( seeker = _head; seeker != NULL ; seeker = seeker->next)
	{
		for( int i = 0; i < _packet_size; i++)
		{
			Serial.print( seeker->packet[ i ] );
			Serial.print( token );
		}
		Serial.println();
	}
}

//Private methods

//Utility methods
bool Robot_CLI::_establishContact()
{
	//Doesn't leave until contact has been established
	while ( Serial.available() <= 0 )
	{
		delay(300);
	}

	return true;//can't ever be false
}

void Robot_CLI::_delCmdLst()
{
	node delNode;

	for(_current = _head->next; _current != NULL; ) //increment ommitted 
	{
		delNode = _current;
		_current = _current->next;
		free( delNode );
	}

	_current = _tail = _head;
	for(int i = 0; _packet_size >= i; i++)
	{
		_head->packet[ i ] = -1;//Set all values packet to -1.
	}
	_head->next = NULL;
	_head->nodeNum = 0;
}
